from launch.actions import GroupAction
from launch_ros.actions import PushRosNamespace

import os

import sys
import yaml
import json


from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch import LaunchDescription
from launch_ros.actions import Node
import os
import yaml

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node

from launch_ros.descriptions import ComposableNode
from launch_ros.actions import ComposableNodeContainer, Node
from launch.actions import TimerAction, Shutdown
from launch import LaunchDescription

def generate_launch_description():
   
    robot_type_params = yaml.safe_load(open(os.path.join(
        get_package_share_directory('rm_vision_bringup'), 'config', 'robot_type.yaml')))

    all_config = []

    # 需要两组的
    all_suffix = ["up", "down"]

    launch_params = yaml.safe_load(open(os.path.join(
        get_package_share_directory('rm_vision_bringup'), 'config', 'launch_params.yaml')))
    robot_type_params = yaml.safe_load(open(os.path.join(
        get_package_share_directory('rm_vision_bringup'), 'config', 'robot_type.yaml')))
    shoot_caliberate_params = yaml.safe_load(open(os.path.join(
        get_package_share_directory('rm_vision_bringup'), 'config', 'shoot_caliberate.yaml')))
        
    node_params = os.path.join(
        get_package_share_directory('rm_vision_bringup'), 'config', 'node_params.yaml')

    for suffix in all_suffix:
        robot_type = "sentry_" + suffix
        this_robot_tfs = launch_params[robot_type]
        my_gun_calibs = shoot_caliberate_params[robot_type]
        # IMU节点
        imuportname = "/dev/ttyUSBimu" + suffix
        # imu_nodes = IncludeLaunchDescription(
        #     PythonLaunchDescriptionSource([os.path.join(
        #         get_package_share_directory('fdilink_ahrs'), 'launch'),
        #         '/odom_tf.launch.py']),

        #     )
        
        
        
        ############ all.launch.py
        

        # detecting 节点
        # nohardware_nodes = IncludeLaunchDescription(
        #     PythonLaunchDescriptionSource([os.path.join(
        #         get_package_share_directory('rm_vision_bringup'), 'launch'),
        #         '/no_hardware.launch.py']),
        #     )

        # rs_node = IncludeLaunchDescription(
        #     PythonLaunchDescriptionSource([os.path.join(
        #         get_package_share_directory('realsense2_camera'), 'launch'),
        #         '/rs_launch.py']),
        #     launch_arguments={'rgb_camera.profile': '848x480x60', 'rgb_camera.exposure': '30', 'rgb_camera.auto_exposure_priority': 'false', 'rgb_camera.enable_auto_exposure': 'false'}.items()
        # )

        # imu_nodes = IncludeLaunchDescription(
        #     PythonLaunchDescriptionSource([os.path.join(
        #         get_package_share_directory('fdilink_ahrs'), 'launch'),
        #         '/odom_tf.launch.py']),
        #     )
        driver_node = IncludeLaunchDescription(
            PythonLaunchDescriptionSource([os.path.join(
                get_package_share_directory('fdilink_ahrs'), 'launch'),
                '/ahrs_driver.launch.py']),
            launch_arguments={"port_name":imuportname}.items()
        )


        rpyparams = this_robot_tfs['imu2gimbal_link']['rpy']
        # 去除两边的引号，并通过空格分割，转化为小数
        rpyparams = rpyparams.strip('\"').split(' ')
        rpyparams = [float(i) for i in rpyparams]
        # print(rpyparams)

        neck_link_xyz_params = this_robot_tfs['gim_org_t_in_neck_link']
        # 去除两边的引号，并通过空格分割，转化为小数
        neck_link_xyz_params = neck_link_xyz_params.strip('\"').split(' ')
        neck_link_xyz_params = [float(i) for i in neck_link_xyz_params]

        odom_tf=Node(
            package="fdilink_ahrs",
            executable="dynamic_frame_tf2_broadcaster",
            name='dynamic_broadcaster',
            parameters=[{'imu_topic':'imu',
                        #  roll pitch yaw
                        'roll': rpyparams[0],
                        'pitch': rpyparams[1],
                        'yaw': rpyparams[2],
                        'neck_x': neck_link_xyz_params[0], 
                        'neck_y': neck_link_xyz_params[1], 
                        'neck_z': neck_link_xyz_params[2],
            }],
        )
        
        
        

        # get robot_type.yaml and shoot_caliberate.yaml to serial_driver_node
        # robot_type_params = os.path.join(
        #     get_package_share_directory('rm_vision_bringup'), 'config', 'robot_type.yaml')
        # shoot_caliberate_params = os.path.join(
        #     get_package_share_directory('rm_vision_bringup'), 'config', 'shoot_caliberate.yaml')


        

        my_runtime = robot_type_params["running_on"]

        if my_runtime == "nx":
            print("Using on nx")
            nx_node = IncludeLaunchDescription(
                PythonLaunchDescriptionSource([os.path.join(
                    get_package_share_directory('nx_processor'), 'launch'),
                    '/launch.py']),
                )



        anti_spinner_node = Node(
            package='anti_spinner',
            executable='anti_spinner_node',
            name='anti_spinner',
            output='both',
            parameters=[my_gun_calibs],
            on_exit=Shutdown(),
            # ros_arguments=['--ros-args', '--log-level',
            #                'serial_driver:='+launch_params['serial_log_level']],
        )

        my_runtime = robot_type_params["running_on"]
        our_color = robot_type_params["our_color"]
        detector_node = Node(
            package='armor_detector',
            executable='armor_detector_node',
            emulate_tty=True,
            output='both',
            parameters=[node_params, {'running_on': my_runtime}, {'our_color': our_color}],
            # arguments=['--ros-args', '--log-level',
            #            'armor_detector:='+launch_params['detector_log_level']],
        )


        should_run_node = [
            # nohardware_nodes,
            driver_node,
            odom_tf,
            detector_node,
            # rs_node,
            # imu_nodes,
            # delay_serial_node,
            anti_spinner_node,
        ]

        if my_runtime == "nx":
            should_run_node.append(nx_node)

        # return LaunchDescription(should_run_node)

        # tracking 节点

        # 自瞄和反小陀螺节点
        

        # 添加名称空间
        this_with_namespace = GroupAction([
            PushRosNamespace(robot_type),
            *should_run_node
        ])
        all_config.append(this_with_namespace)
        # pass

    # 需要一组的
        
    # 串口通信节点
    serial_driver_node = Node(
        package='rm_serial_driver',
        executable='rm_serial_driver_node',
        name='serial_driver',
        output='both',
        emulate_tty=True,
        on_exit=Shutdown(),
        # ros_arguments=['--ros-args', '--log-level',
        #                'serial_driver:='+launch_params['serial_log_level']],
    )

    delay_serial_node = TimerAction(
        period=1.5,
        actions=[serial_driver_node],
    )
    all_config.append(delay_serial_node)


    # 无下位机控制节点


    return LaunchDescription(all_config)
    # if me == "sentry_up_and_down":
    #     print("Double gimbal")
    #     all_one_up = IncludeLaunchDescription(
    #         PythonLaunchDescriptionSource([os.path.join(
    #             get_package_share_directory('rm_vision_bringup'), 'launch'),
    #             '/no_hardware.launch.py']),
    #             launch_arguments={"robot_type": "sentry_up"}.items(),
    #         )
    #     all_one_down = IncludeLaunchDescription(
    #         PythonLaunchDescriptionSource([os.path.join(
    #             get_package_share_directory('rm_vision_bringup'), 'launch'),
    #             '/no_hardware.launch.py']),
    #             launch_arguments={"robot_type": "sentry_down"}.items(),
    #         )
    
    #     all_up_with_namespace = GroupAction(
    #         actions=[
    #             PushRosNamespace("sentry_up"),
    #             all_one_up,
    #         ]
    #     )

    #     all_down_with_namespace = GroupAction(
    #         actions=[
    #             PushRosNamespace('sentry_down'),
    #             all_one_down,
    #         ]
    #     )

    #     return LaunchDescription([
    #         all_up_with_namespace, all_down_with_namespace,
    #     ])
    # else:
    #     print("single gimbal")
    #     all_me = IncludeLaunchDescription(
    #         PythonLaunchDescriptionSource([os.path.join(
    #             get_package_share_directory('rm_vision_bringup'), 'launch'),
    #             '/no_hardware.launch.py']),
    #             launch_arguments={"robot_type": me}.items()
    #         )
        
    #     return LaunchDescription([
    #         all_me,
    #     ])

    # allone = IncludeLaunchDescription(
    #     PythonLaunchDescriptionSource([os.path.join(
    #         get_package_share_directory('rm_vision_bringup'), 'launch'),
    #         '/no_hardware.launch.py'])
    #     )
   